// actoin: /navigate_to_pose [nav2_msgs/action/NavigateToPose]

#include "rclcpp/rclcpp.hpp"
#include "rclcpp_action/rclcpp_action.hpp"
#include "nav2_msgs/action/navigate_to_pose.hpp"

using namespace std::chrono_literals;

class Nav2Point : public rclcpp::Node
{
private:
    rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SharedPtr nav2point_client_;

public:
    Nav2Point() : Node("nav_2_point_node")
    {
        nav2point_client_ = rclcpp_action::create_client<nav2_msgs::action::NavigateToPose>(this, "/navigate_to_pose");
    }
    void send_point()
    {
        if (!nav2point_client_->wait_for_action_server(5s))
        {
            RCLCPP_INFO(this->get_logger(), "等待动作服务器/navigate_to_pose启动超时！");
        }
        auto goal = nav2_msgs::action::NavigateToPose::Goal();
        goal.pose.header.frame_id = "map";
        goal.pose.header.stamp = this->get_clock()->now();
        goal.pose.pose.position.x = 1.;
        goal.pose.pose.position.y = 0.;
        goal.pose.pose.orientation.w = 1.;

        rclcpp_action::Client<nav2_msgs::action::NavigateToPose>::SendGoalOptions option;
        option.feedback_callback = std::bind(&Nav2Point::Feedback_Callback, this, std::placeholders::_1, std::placeholders::_2);
        option.goal_response_callback = std::bind(&Nav2Point::Goal_Resbonse_Callback, this, std::placeholders::_1);
        option.result_callback = std::bind(&Nav2Point::Result_Callback, this, std::placeholders::_1);
        nav2point_client_->async_send_goal(goal, option);
    }
    void Feedback_Callback(rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::SharedPtr handle,
                           const std::shared_ptr<const nav2_msgs::action::NavigateToPose::Feedback> feedback)
    {
        (void)handle;
        RCLCPP_INFO(this->get_logger(),
                    "当前坐标（x:%.2f,y:%.2f）",
                    feedback->current_pose.pose.position.x,
                    feedback->current_pose.pose.position.y);
    }
    void Goal_Resbonse_Callback(rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::SharedPtr handle)
    {
        if (handle)
        {
            RCLCPP_INFO(this->get_logger(), "动作服务器接收!");
        }
        else if (!handle)
        {
            RCLCPP_INFO(this->get_logger(), "动作服务器拒绝接收！");
        }
    }
    void Result_Callback(const rclcpp_action::ClientGoalHandle<nav2_msgs::action::NavigateToPose>::WrappedResult &result)
    {
        switch (result.code)
        {
        case rclcpp_action::ResultCode::SUCCEEDED:
            RCLCPP_INFO(this->get_logger(), "导航成功！");
            break;
        case rclcpp_action::ResultCode::CANCELED:
            RCLCPP_INFO(this->get_logger(), "导航被取消！");
        case rclcpp_action::ResultCode::ABORTED:
            RCLCPP_INFO(this->get_logger(), "导航被中断！");          
        default:
            RCLCPP_INFO(this->get_logger(), "未知错误！");
            break;
        }
    }
};

int main(int argc, char *argv[])
{
    rclcpp::init(argc, argv);
    auto node = std::make_shared<Nav2Point>();
    node->send_point();
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}
